/* 
 * File:   Track.cpp
 * Author: dave
 * 
 * Created on 2010年11月2日, 下午5:03
 */
#include "Track.h"

extern int numFrame;
extern int hdims;
extern int minT;


Track::Track(Camera *camera) {
    this->cam = camera;
    maxSize = 20;
    deadfrm = 10;
}

Track::Track(const Track& orig) {
}

Track::~Track() {
}

//初始化Track类
void Track::Init()
{
    float hranges_arr[]={0,180};
    float *hranges = hranges_arr;
    int hdims = 256;

    id = 0;objSize = 0;
    for(int i = 0; i < maxSize; ++i)
    {
        V[i].hist = cvCreateHist( 1, &hdims, CV_HIST_ARRAY, &hranges, 1 );
        V[i].image = cvCreateImage(cvGetSize(cam->pFrame), 8, 3);
        V[i].mask = cvCreateImage(cvGetSize(cam->pFrame), 8, 1);
        V[i].hsv = cvCreateImage( cvGetSize(cam->pFrame), 8, 3 );
        V[i].hue = cvCreateImage( cvGetSize(cam->pFrame), 8, 1 );
        V[i].backProject = cvCreateImage( cvGetSize(cam->pFrame), 8, 1 );
    }
}

//判断新物体comp是否被包含，废弃
bool Track::Match(CvConnectedComp comp)
{
    for(int i = 0; i < objSize; ++i)
    {
        if(comp.rect.x > V[i].comp.rect.x + 10 || V[i].comp.rect.x > comp.rect.x + 10)
            continue;
        if(comp.rect.y > V[i].comp.rect.y + 10 || V[i].comp.rect.y > comp.rect.y + 10)
            continue;
        if(comp.area > V[i].comp.area * 2 || V[i].comp.area > comp.area * 2)
        if(comp.rect.height * comp.rect.width > V[i].comp.rect.height * V[i].comp.rect.width * 2
            || V[i].comp.rect.height * V[i].comp.rect.width > comp.rect.height * comp.rect.width * 2)
            continue;
        return true;
    }
    return false;
}

//判断矩形a，b质心相互包含
bool Track::Match(CvRect a, CvRect b)
{
    int dis = a.x - b.x;
    int len;
    if(dis < 0) dis = -dis;
    len = (a.width + b.width) / 4;
    if(len < 20) len = 20;
    if(dis > len)
        return false;
    dis = a.y - b.y;
    if(dis < 0) dis = -dis;
    len = (a.height + b.height) / 4;
    if(len < 20) len = 20;
    if(dis > len)
        return false;

    if(a.height * a.width > b.height * b.width * 2
            || b.height * b.width > a.height * a.width * 2)
        return false;
    return true;
}

//判断新物体obj是否被包含
bool Track::Match(trackObject obj)
{
    int x = (obj.pt0.x + obj.pt1.x) / 2, y = (obj.pt0.y + obj.pt1.y) / 2;
    for(int i = 0; i < objSize; ++i)
    {
        int X = (V[i].pt0.x + V[i].pt1.x) / 2, Y = (V[i].pt0.y + V[i].pt1.y) / 2;
        if((x < V[i].pt0.x || x > V[i].pt1.x || y < V[i].pt0.y || y > V[i].pt1.y) &&
                (X < obj.pt0.x || X > obj.pt1.x || Y < obj.pt0.y || Y > obj.pt1.y))
            continue;
        V[i].pt0 = obj.pt0;
        V[i].pt1 = obj.pt1;
        V[i].rect = obj.rect;
        V[i].matched = true;
        return true;
    }
    return false;
}

//检查跟踪结果是否合法
bool Track::Check(CvConnectedComp* comp, CvBox2D* box)
{
    if(comp->rect.width <= 0 || comp->rect.height <= 0
            || box->size.width <= 0 || box->size.height <= 0
            || box->center.x < 0 || box->center.x >= cam->pFrame->width
            || box->center.y < 0 || box->center.y >= cam->pFrame->height)
        return false;
    return true;
}

//新物体跟踪，废弃
void Track::NewObject(vector<trackObject> obj)
{
//    IplImage *image = cvCreateImage(cvGetSize(cam->pFrame), 8, 3);
//    IplImage *mask = cvCreateImage(cvGetSize(cam->pFrame), 8, 1);
//
//    cvCopy(cam->pPre, image, 0);
//    cvCvtColor(image, cam->hsv, CV_BGR2HSV);
//    cvInRangeS(cam->hsv, cvScalar(0, 30, 10, 0),
//            cvScalar(180, 256, 256,0), mask );
//    cvSplit(cam->hsv, cam->hue, 0, 0, 0);
//
//    for(int i = 0; i < obj.size(); ++i)
//    {
//        //搜索边框
//        window = cvRect(obj[i].pt0.x, obj[i].pt0.y,
//                obj[i].pt1.x - obj[i].pt0.x + 1, obj[i].pt1.y - obj[i].pt0.y + 1);
//        //设置图像的感兴趣区域
//        cvSetImageROI(cam->hue, window);
//        cvSetImageROI(mask, window);
//        cvCalcHist(&cam->hue, cam->hist, 0, mask);
//        float max_val = 0.0;
//        cvGetMinMaxHistValue(cam->hist, 0, &max_val, 0, 0 );
//        cvConvertScale(cam->hist->bins, cam->hist->bins,
//                max_val ? 255. / max_val : 0, 0 );
//        cvResetImageROI(cam->hue);
//        cvResetImageROI(mask);
//
//        //计算目标直方图的反向投影
//        try {
//            cvCalcBackProject(&(cam->hue), cam->backProject, cam->hist);
//        } catch(...) {
//
//        }
//
//        //蒙板
//        cvAnd(cam->backProject, mask, cam->backProject, 0);
//        //执行CamShift算法跟踪目标
//        cvCamShift(cam->backProject, window,
//                cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
//                &comp, &box);
//
//        if(!Match(comp))
//        {
//            trackObject tmpObj = obj[i];
//            tmpObj.id = id++;
//            tmpObj.lostfrm = 0;
//            tmpObj.comp.rect = window;
//            tmpObj.comp.area = window.height * window.width;
//            //保存跟踪结果
//            V.push_back(tmpObj);
//        }
//    }
//    cvReleaseImage(&image);
//    cvReleaseImage(&mask);
}

//添加新物体到跟踪列表
void Track::Add(trackObject obj)
{
    V[objSize].id = id++;
    V[objSize].lostfrm = 0;
    V[objSize].matched = true;
    V[objSize].rect = obj.rect;
    V[objSize].pt0 = obj.pt0;
    V[objSize].pt1 = obj.pt1;
    objSize++;
}

//丢弃跟踪丢失的物体
void Track::Erase()
{
    int i = 0, j = 0;
    for( ; i < objSize; ++i)
    {
        if(V[i].lostfrm < deadfrm)
        {
            if(i != j)
            {
                V[j].id = V[i].id;
                V[j].lostfrm = V[i].lostfrm;
                V[j].matched = V[i].matched;
                V[j].pt0 = V[i].pt0;
                V[j].pt1 = V[i].pt1;
                V[j].rect = V[i].rect;
            }
            ++j;
        }
    }
    objSize = j;
}

//对指定物体进行CamShift跟踪
void Track::CamShift(trackObject * obj)
{
    cvCopy(cam->pFrame, obj->image, 0);
    cvCvtColor(obj->image, obj->hsv, CV_BGR2HSV);
    cvInRangeS(obj->hsv, cvScalar(0, 30, 10, 0),
            cvScalar(180, 256, 256,0), obj->mask );
    cvSplit(obj->hsv, obj->hue, 0, 0, 0);
    
    //设置图像的感兴趣区域
    cvSetImageROI(obj->hue, obj->rect);
    cvSetImageROI(obj->mask, obj->rect);

    cvCalcHist(&obj->hue, obj->hist, 0, obj->mask);
    float max_val = 0.0;
    cvGetMinMaxHistValue(obj->hist, 0, &max_val, 0, 0 );
    cvConvertScale(obj->hist->bins, obj->hist->bins,
            max_val ? 255. / max_val : 0, 0 );
    cvResetImageROI(obj->hue);
    cvResetImageROI(obj->mask);
    //计算目标直方图的反向投影
    cvCalcBackProject(&(obj->hue), obj->backProject, obj->hist);
    
    //蒙板
    cvAnd(obj->backProject, obj->mask, obj->backProject, 0);
    //执行CamShift算法跟踪目标
    cvCamShift(obj->backProject, obj->rect,
            cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
            &obj->comp, &obj->box);
}

//处理新检测物体
void Track::TrackNew(vector<trackObject> obj)
{
    for(int i = 0; i < obj.size(); ++i)
    {
        if(!Match(obj[i]))
        {
//            if(obj[i].pt1.x < obj[i].pt0.x || obj[i].pt1.y < obj[i].pt0.y)
//                printf("Error:%d %d %d %d %d\t", obj[i].id, obj[i].pt0.x, obj[i].pt0.y, obj[i].pt1.x, obj[i].pt1.y);

            obj[i].matched = false;
            //保存跟踪结果
            //Add(obj[i]);
            printf("%d %d %d %d %d\t", obj[i].id, obj[i].pt0.x, obj[i].pt0.y, obj[i].pt1.x, obj[i].pt1.y);
            printf("%d %d %d %d\n", obj[i].rect.x, obj[i].rect.y, obj[i].rect.width, obj[i].rect.height);
        } else {
            obj[i].matched = true;
        }
    }
}

//跟踪目标物体
void Track::TrackExist(vector<trackObject> obj)
{
    int i, k = objSize;
    for(i = 0; i < objSize; ++i)
    {
        CamShift(&V[i]);
    }

    for(i = 0; i < objSize; ++i)
    {
        if(Check(&V[i].comp, &V[i].box) && Match(V[i].comp.rect, V[i].rect))
        {
            V[i].pt0 = cvPoint(V[i].comp.rect.x, V[i].comp.rect.y);
            V[i].pt1 = cvPoint(V[i].comp.rect.x + V[i].comp.rect.width - 1,
                    V[i].comp.rect.y + V[i].comp.rect.height - 1);
            V[i].rect = V[i].comp.rect;
            V[i].lostfrm = 0;
        } else {
            V[i].lostfrm++;
        }
        V[i].matched = false;
    }
    printf("objSize before Add: %d\n", objSize);

    for(int i = 0; i < obj.size(); ++i)
    {
        if(!obj[i].matched && Match(obj[i]))
        {
            obj[i].matched = true;
        }
    }
    for(i = 0; i < obj.size(); ++i)
    {
        if(!obj[i].matched)
        {
            Add(obj[i]);
        }
    }
    printf("objSize after Add: %d\n", objSize);
    for(i = k; i < objSize; ++i)
    {
        CamShift(&V[i]);
    }

    for(i = k; i < objSize; ++i)
    {
        if(Check(&V[i].comp, &V[i].box) && Match(V[i].comp.rect, V[i].rect))
        {
            V[i].pt0 = cvPoint(V[i].comp.rect.x, V[i].comp.rect.y);
            V[i].pt1 = cvPoint(V[i].comp.rect.x + V[i].comp.rect.width - 1,
                    V[i].comp.rect.y + V[i].comp.rect.height - 1);
            V[i].rect = V[i].comp.rect;
            V[i].lostfrm = 0;
        } else {
            V[i].lostfrm++;
        }
        V[i].matched = false;
    }
}

//Track类程序入口
void Track::Run(vector<trackObject> obj)
{
    //NewObject(obj);
    TrackNew(obj);

    TrackExist(obj);

    Erase();

    printf("TotalObj:%d\t\n", objSize);
}

//在画面中标示跟踪物体
void Track::Draw(IplImage * image, CvScalar color)
{
    char s[8];

    CvFont * font = &cvFont(2);
    for(int i = 0; i < objSize; ++i)
    {
//        if(V[i].lostfrm) continue;
        printf("id:%d\tlostfrm:%d\t%d %d\n", V[i].id, V[i].lostfrm, V[i].pt1.x, V[i].pt1.y);
        cvRectangle(image, V[i].pt0, V[i].pt1, color, 2);
        sprintf(s, "%d", V[i].id);
        cvPutText(image, s, V[i].pt1, font, color);
    }
}